#include "stdafx.h"
#include "Frustum.h"
#include "BoundingSphere.h"

using namespace mmath;

void FrustumPersp::construct(const XMVECTOR & position, const XMVECTOR & forward, const XMVECTOR & up, const XMVECTOR & right, float fov, float aspectRatio, float nearZ, float farZ)
{
	XMStoreFloat3(&Position, position);
	XMStoreFloat3(&Forward, forward);
	XMStoreFloat3(&Up, up);
	XMStoreFloat3(&Right, right);
	FieldOfView = fov;
	AspectRatio = aspectRatio;
	NearZ = nearZ;
	FarZ = farZ;

	// planes
	XMVECTOR nearPt = XMVectorAdd(position, XMVectorMultiply(forward, XMVectorReplicate(nearZ))); // intersection of lookAt and near plane
	XMStoreFloat4(&Planes[0], XMPlaneFromPointNormal(nearPt, forward));

	XMVECTOR farPt = XMVectorAdd(position, XMVectorMultiply(forward, XMVectorReplicate(farZ))); // intersection of lookAt and far plane
	XMStoreFloat4(&Planes[1], XMPlaneFromPointNormal(farPt, XMVectorNegate(forward)));

	float nearHalfHei = tanf(fov * 0.5f) * nearZ;
	float nearHalfWid = nearHalfHei * aspectRatio;

	float farHalfHei = tanf(fov * 0.5f) * farZ;
	float farHalfWid = farHalfHei * aspectRatio;

	XMVECTOR rightTan = XMVectorSubtract(
		XMVectorAdd(nearPt, XMVectorMultiply(right, XMVectorReplicate(nearHalfWid))),
		position
	);
	rightTan = XMVector3Normalize(rightTan);
	XMVECTOR rightNrm = XMVector3Cross(up, rightTan);
	XMStoreFloat4(&Planes[2], XMPlaneFromPointNormal(position, XMVectorNegate(rightNrm)));

	XMVECTOR leftTan = XMVectorSubtract(
		XMVectorAdd(nearPt, XMVectorMultiply(XMVectorNegate(right), XMVectorReplicate(nearHalfWid))),
		position
	);
	leftTan = XMVector3Normalize(leftTan);
	XMVECTOR leftNrm = XMVector3Cross(up, leftTan);
	XMStoreFloat4(&Planes[3], XMPlaneFromPointNormal(position, /*XMVectorNegate*/(leftNrm)));

	XMVECTOR topTan = XMVectorSubtract(
		XMVectorAdd(nearPt, XMVectorMultiply(up, XMVectorReplicate(nearHalfHei))),
		position
	);
	topTan = XMVector3Normalize(topTan);
	XMVECTOR topNrm = XMVector3Cross(right, topTan);
	XMStoreFloat4(&Planes[4], XMPlaneFromPointNormal(position, /*XMVectorNegate*/(topNrm)));

	XMVECTOR bottomTan = XMVectorSubtract(
		XMVectorAdd(nearPt, XMVectorMultiply(XMVectorNegate(up), XMVectorReplicate(nearHalfHei))),
		position
	);
	bottomTan = XMVector3Normalize(bottomTan);
	XMVECTOR bottomNrm = XMVector3Cross(right, bottomTan);
	XMStoreFloat4(&Planes[5], XMPlaneFromPointNormal(position, XMVectorNegate(bottomNrm)));

	// corners
	XMVECTOR tmp;

	// near
	tmp = XMVectorAdd(
		XMVectorAdd(nearPt, XMVectorMultiply(right, XMVectorReplicate(nearHalfWid))),
		XMVectorMultiply(up, XMVectorReplicate(nearHalfHei))
	);
	XMStoreFloat3(&Points[0], tmp);

	tmp = XMVectorSubtract(tmp, XMVectorMultiply(right, XMVectorReplicate(nearHalfWid * 2)));
	XMStoreFloat3(&Points[1], tmp);

	tmp = XMVectorSubtract(tmp, XMVectorMultiply(up, XMVectorReplicate(nearHalfHei * 2)));
	XMStoreFloat3(&Points[2], tmp);

	tmp = XMVectorAdd(tmp, XMVectorMultiply(right, XMVectorReplicate(nearHalfWid * 2)));
	XMStoreFloat3(&Points[3], tmp);

	// far
	tmp = XMVectorAdd(
		XMVectorAdd(farPt, XMVectorMultiply(right, XMVectorReplicate(farHalfWid))),
		XMVectorMultiply(up, XMVectorReplicate(farHalfHei))
		);
	XMStoreFloat3(&Points[4], tmp);

	tmp = XMVectorSubtract(tmp, XMVectorMultiply(right, XMVectorReplicate(farHalfWid * 2)));
	XMStoreFloat3(&Points[5], tmp);

	tmp = XMVectorSubtract(tmp, XMVectorMultiply(up, XMVectorReplicate(farHalfHei * 2)));
	XMStoreFloat3(&Points[6], tmp);

	tmp = XMVectorAdd(tmp, XMVectorMultiply(right, XMVectorReplicate(farHalfWid * 2)));
	XMStoreFloat3(&Points[7], tmp);
}

void FrustumOrtho::construct( const XMVECTOR & position, const XMVECTOR & forward, const XMVECTOR & up, const XMVECTOR & right, float width, float height, float nearZ, float farZ )
{
	XMStoreFloat3(&Position, position);
	XMStoreFloat3(&Forward, forward);
	XMStoreFloat3(&Up, up);
	XMStoreFloat3(&Right, right);
	Height = height;
	Width = width;
	NearZ = nearZ;
	FarZ = farZ;

	// planes
	XMVECTOR nearPt = XMVectorAdd(position, forward * nearZ); // intersection of lookAt and near plane
	XMStoreFloat4(&Planes[0], XMPlaneFromPointNormal(nearPt, forward));

	XMVECTOR farPt = XMVectorAdd(position, forward * farZ); // intersection of lookAt and far plane
	XMStoreFloat4(&Planes[1], XMPlaneFromPointNormal(farPt, -forward));

	XMVECTOR nearTopRightPt = nearPt + up * height * 0.5f + right * width * 0.5f;
	XMVECTOR nearBottomLeftPt = nearPt - up * height * 0.5f - right * width * 0.5f;
	XMStoreFloat4(&Planes[2], XMPlaneFromPointNormal(nearTopRightPt, -right)); // right
	XMStoreFloat4(&Planes[3], XMPlaneFromPointNormal(nearBottomLeftPt, right)); // left
	XMStoreFloat4(&Planes[4], XMPlaneFromPointNormal(nearTopRightPt, -up)); // top
	XMStoreFloat4(&Planes[5], XMPlaneFromPointNormal(nearBottomLeftPt, up)); // bottom

	// points
	XMVECTOR halfWid = right * width * 0.5f;
	XMVECTOR halfHei = up * height * 0.5f;
	// near
	XMStoreFloat3(&Points[0], nearPt + halfWid + halfHei);
	XMStoreFloat3(&Points[1], nearPt - halfWid + halfHei);
	XMStoreFloat3(&Points[2], nearPt - halfWid - halfHei);
	XMStoreFloat3(&Points[3], nearPt + halfWid - halfHei);
	// far
	XMStoreFloat3(&Points[4], farPt + halfWid + halfHei);
	XMStoreFloat3(&Points[5], farPt - halfWid + halfHei);
	XMStoreFloat3(&Points[6], farPt - halfWid - halfHei);
	XMStoreFloat3(&Points[7], farPt + halfWid - halfHei);
}

bool mmath::frustumSphereintersects( const XMFLOAT4 * planes, const BoundingSphere & sphere )
{
	float distance;
	//bool result = true;

	XMVECTOR center = XMLoadFloat3(&sphere.Center);

	for(uint i = 0; i < 6; i++)
	{
		XMVECTOR plane = XMLoadFloat4(&planes[i]);
		distance = XMVectorGetX(
			XMVectorAdd(
			XMVector3Dot(center, plane),
			XMVectorSplatW(plane)
			)
			);
		if (distance < -sphere.Radius)
			return false;
		//else if (distance < sphere.Radius)
		//	result = true;
	}
	return true;
}

mmath::BoundingSphere mmath::getFrustumBoundingSphere( const XMFLOAT3 * points )
{
	BoundingSphere sphere;
	// frustum's bounding sphere is constructed from opposite points on near and far plane of the frustum
	XMFLOAT3 diagonalCuttingPlanePoints[4] = { points[0], points[2], points[4], points[6] };
	sphere.construct((byte*)&diagonalCuttingPlanePoints, 4, 12);
	return sphere;
}
